#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/empty.hpp"
#include "std_msgs/msg/bool.hpp"
#include "std_msgs/msg/float32.hpp"
#include "customized_msgs/msg/gcs_cmd.hpp"
#include "customized_msgs/msg/arg_set_length.hpp"
#include <Eigen/Dense>
#include <string>
#include <vector>

class GcsClient : public rclcpp::Node
{
public:
    GcsClient() : Node("gcs_client_node")
    {
        // Initialize subscribers
        gcs_cmd_sub = this->create_subscription<customized_msgs::msg::GcsCmd>(
            "/winch_wireless_server/gcs_cmd", 10, std::bind(&GcsClient::GcsCmdCallback, this, std::placeholders::_1));

        // Initialize publishers
        winch_length_pub = this->create_publisher<customized_msgs::msg::ArgSetLength>("/winch_wireless_server/set_length", 10);
        winch_virtual_rc_pub = this->create_publisher<std_msgs::msg::Float32>("/gcs_client_winch/virtual_rc", 10);
        winch_zero_pub = this->create_publisher<std_msgs::msg::Empty>("/winch_wireless_server/set_zero", 10);
        winch_near_ground_pub = this->create_publisher<std_msgs::msg::Bool>("/winch_ctrl/rappel_near_ground", 10);
        winch_touch_ground_pub = this->create_publisher<std_msgs::msg::Bool>("/winch_ctrl/rappel_touch_ground", 10);
        winch_precise_zero_pub = this->create_publisher<std_msgs::msg::Bool>("/winch_ctrl/precise_zero", 10);
        motor_reset_pub = this->create_publisher<std_msgs::msg::Empty>("/winch_wireless_server/motor_reset", 10);
        winch_catch_up_pub = this->create_publisher<std_msgs::msg::Bool>("/winch_ctrl/catch_up", 10);


        // Initialize timer
        winch_timer = this->create_wall_timer(
            std::chrono::milliseconds(static_cast<int>(1000.0 / winch_frequency)), std::bind(&GcsClient::WinchTimerCallback, this));

        RCLCPP_WARN(this->get_logger(), "GCS client node is initialized");
    }

private:
    rclcpp::Subscription<customized_msgs::msg::GcsCmd>::SharedPtr gcs_cmd_sub;
    rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr winch_zero_pub;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr winch_near_ground_pub;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr winch_touch_ground_pub;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr winch_precise_zero_pub;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr winch_catch_up_pub;

    rclcpp::Publisher<customized_msgs::msg::ArgSetLength>::SharedPtr winch_length_pub;
    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr winch_virtual_rc_pub;
    rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr motor_reset_pub;

    Eigen::Vector3d odom_data = Eigen::Vector3d::Zero();
    double voltage_data = 0.0;

    bool winch_near_ground_flag = false;
    bool winch_touch_ground_flag = false;
    bool precise_zero_flag = false;
    bool winch_catch_up_flag = false;
    rclcpp::TimerBase::SharedPtr winch_timer;
    double winch_frequency = 10.0;
    double winch_speed = 0.0; 
    rclcpp::Time last_update_winch_speed_time = this->get_clock()->now();
    double time_out = 1.0;
    double winch_maunal_speed = 0.5;

    bool winch_speed_init_flag = false;
    bool record_bag_trigger_flag = true;
    rclcpp::Time last_receive_gcs_cmd_time = this->get_clock()->now();

    std::vector<std::string> gcs_cmd_list = {"restart_program","winch_zero","winch_near_ground","takeoff","winch_touch_ground","winch_catch_up","reset_mission","winch_go_zero","precise_zero","grabber_open","grabber_close","reset_winch","kill_switch","record_bag"};

    void WinchTimerCallback()
    {
        double gcs_time_out = 1.0;
        double elapsed_time = (this->get_clock()->now() - last_receive_gcs_cmd_time).seconds();
        if (elapsed_time > gcs_time_out)
        {
            // RCLCPP_WARN(this->get_logger(), "gcs_client_node_winch: no gcs cmd received for %f secs, stop winch", elapsed_time);
            winch_near_ground_flag = false;
            winch_touch_ground_flag = false;
            precise_zero_flag = false;
            winch_speed = 0.0;
        }

        if (true)
        {
            rclcpp::Time current_time = this->get_clock()->now();
            double dt = (current_time - last_update_winch_speed_time).seconds();
            if (dt > time_out)
            {
                if (winch_speed_init_flag)
                {
                    RCLCPP_ERROR(this->get_logger(), "winch speed timeout for %f secs!!", dt);
                    std_msgs::msg::Float32 winch_virtual_rc_msg;
                    winch_virtual_rc_msg.data = 0.0;
                    winch_virtual_rc_pub->publish(winch_virtual_rc_msg);
                }
            }
            else
            {
                std_msgs::msg::Float32 winch_virtual_rc_msg;
                winch_virtual_rc_msg.data = winch_speed;
                winch_virtual_rc_pub->publish(winch_virtual_rc_msg);
            }
        }

        std_msgs::msg::Bool winch_near_ground_msg;
        if (winch_near_ground_flag){
            winch_near_ground_msg.data = winch_near_ground_flag;
            winch_near_ground_pub->publish(winch_near_ground_msg);
            winch_near_ground_flag=false;
        }
       

        std_msgs::msg::Bool winch_touch_ground_msg;
        winch_touch_ground_msg.data = winch_touch_ground_flag;
        winch_touch_ground_pub->publish(winch_touch_ground_msg);

        std_msgs::msg::Bool precise_zero_msg;
        precise_zero_msg.data = precise_zero_flag;
        winch_precise_zero_pub->publish(precise_zero_msg);
    
        std_msgs::msg::Bool winch_catch_up_msg;
        winch_catch_up_msg.data = winch_catch_up_flag;
        winch_catch_up_pub->publish(winch_catch_up_msg);

    
    }

    void GcsCmdCallback(const customized_msgs::msg::GcsCmd::SharedPtr msg)
    {
        // RCLCPP_INFO(this->get_logger(), "enter gcs cmd cb");
        
        last_receive_gcs_cmd_time = msg->stamp;
        int cmd_id = msg->cmd;
        if (fabs(msg->winch_speed) < 1.01)
        {
            winch_speed = msg->winch_speed;
            if (fabs(winch_speed) > 0.01)
            {
                winch_speed_init_flag = true;
            }
            last_update_winch_speed_time = this->get_clock()->now();
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "winch speed is out of range: %f", msg->winch_speed);
        }

        winch_near_ground_flag = false;
        winch_touch_ground_flag = false;
        precise_zero_flag = false;
        winch_catch_up_flag = false;


        if (cmd_id >= static_cast<int>(gcs_cmd_list.size()))
        {
            if (cmd_id != 100)
                RCLCPP_ERROR(this->get_logger(), "gcs_client_node_winch: receive invalid cmd id: %d", cmd_id);
            return;
        }

        std::string cmd_str = gcs_cmd_list[cmd_id];
        RCLCPP_INFO(this->get_logger(), "receiving gcs cmd: %s", cmd_str.c_str());

        if (cmd_str == "winch_zero")
        {
            std_msgs::msg::Empty empty_msg;
            winch_zero_pub->publish(empty_msg);
        }
        else if (cmd_str == "winch_near_ground")
        {
            winch_near_ground_flag = true;
        }
        else if (cmd_str == "winch_touch_ground")
        {
            winch_touch_ground_flag = true;
        }
        else if (cmd_str == "winch_go_zero")
        {
            customized_msgs::msg::ArgSetLength winch_go_zero_msg;
            winch_go_zero_msg.abs_ctrl = true;
            winch_go_zero_msg.length = 0.0;
            winch_length_pub->publish(winch_go_zero_msg);
        }
        else if (cmd_str == "precise_zero")
        {
            precise_zero_flag = true;
        }
        else if (cmd_str == "reset_winch")
        {
            motor_reset_pub->publish(std_msgs::msg::Empty());
        }
        else if (cmd_str == "winch_catch_up")
        {
            winch_catch_up_flag = true;
        }
        else
        {
            RCLCPP_WARN(this->get_logger(), "invalid cmd:%s", cmd_str.c_str());
        }
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<GcsClient>());
    rclcpp::shutdown();
    return 0;
}
